#ifndef _UTILS_HPP
#define _UTILS_HPP
#include <iostream>
#include <eigen3/Eigen/Dense>
#include <memory>

namespace Nav{

const double PI         = 3.141592653589793;
const double D2R        = 0.017453292519943295;
const double R2D        = 57.29577951308232;

const double START_TIME = 456300.0;
// const double START_TIME = 91620.0;

// WGS-84椭球体参数 
// http://www.celiang.net/article/143
const double OMEGA_E    = 7.2921151467e-5;  // 地球自转角速度 rad/s
const double Ra         = 6378137.0;        // 长轴
const double Rb         = 6356752.3142;     // 短轴
const double E2         = 0.00669437999013; // 第一偏心率e的平方 e=sqrt(a^2-b^2)/a
const double GA         = 9.7803267715;
const double GB         = 9.8321863685;
const double F          = 1.0 / 298.257223563;
const double GM         = 3.986005e14;      // 万有引力常量G * 地球质量M

enum{
    LAT, // 纬度
    LON, // 经度
    ALT, // 高度
};
enum{
    YAW, // 偏航
    PIT, // 俯仰
    ROL, // 横滚
};
enum{
    X,
    Y,
    Z,
};
enum{
    N, // 北
    E, // 东
    D, // 地
};

class ImuData{
public:
    double timestamp;
    Eigen::Vector3d gyro_increm;
    Eigen::Vector3d accel_increm;
};
using ImuDataPtr = std::shared_ptr<ImuData>;

class GnssData{
public:
    bool valid;
    double timestamp;
    Eigen::Vector3d pos;        // 纬、经、高
    Eigen::Vector3d pos_std;    // 
};
using GnssDataPtr = std::shared_ptr<GnssData>;

class Data{
public:
    ImuData  imudata;
    GnssData gnssdata;
};
using DataPtr = std::shared_ptr<Data>;

class ImuModel{
public:
    double ARW;                             // 角度随机游走
    double VRW;                             // 速度随机游走
    double gyro_bias_std;                   // 陀螺仪零偏标准差
    double gyro_bias_corr_time;             // 陀螺仪零偏相关时间
    double accel_bias_std;                  // 加速度计零偏标准差
    double accel_bias_corr_time;            // 加速度计零偏相关时间
    double gyro_scale_factor_std;           // 陀螺仪比例因子标准差
    double gyro_scale_factor_corr_time;     // 陀螺仪比例因子相关时间
    double accel_scale_factor_std;          // 加速度计比例因子标准差
    double accel_scale_factor_corr_time;    // 加速度计比例因子相关时间
public:
    ImuModel(){}
};

class State{
public:
    double timestamp;
    Eigen::Vector3d     pos;    // 纬经高       :rad rad m
    Eigen::Vector3d     pos_std;
    Eigen::Vector3d     vel;    // 北东地       :m/s
    Eigen::Vector3d     vel_std;
    Eigen::Vector3d     att;    // rol pit yaw :rad
    Eigen::Vector3d     att_std;
    Eigen::Matrix3d     rotmat; // 旋转矩阵
    Eigen::Quaterniond  q_nb;   // n->b变换四元数 姿态更新时求解的变换四元数
    Eigen::Quaterniond  q_ne;   // e->n变换四元数
    Eigen::Vector3d     gyro_bias;          // 陀螺仪零偏
    Eigen::Vector3d     accel_bias;         // 加速度计零偏
    Eigen::Vector3d     gyro_scale_factor;  // 陀螺仪比例因子
    Eigen::Vector3d     accel_scale_factor; // 加速度计比例因子

};
using StatePtr = std::shared_ptr<State>;

typedef struct{
    double timestamp;
    Eigen::Vector3d pos;
    Eigen::Vector3d vel;
    Eigen::Vector3d att;
}result_st;

void Euler2Quat(const Eigen::Vector3d& euler, Eigen::Quaterniond& quat){
    double rol = euler[ROL];
    double pit = euler[PIT];
    double yaw = euler[YAW];
    quat.w() = cos(rol / 2) * cos(pit / 2) * cos(yaw / 2) + sin(rol / 2) * sin(pit / 2) * sin(yaw / 2);
    quat.x() = sin(rol / 2) * cos(pit / 2) * cos(yaw / 2) - cos(rol / 2) * sin(pit / 2) * sin(yaw / 2);
    quat.y() = cos(rol / 2) * sin(pit / 2) * cos(yaw / 2) + sin(rol / 2) * cos(pit / 2) * sin(yaw / 2);
    quat.z() = cos(rol / 2) * cos(pit / 2) * sin(yaw / 2) - sin(rol / 2) * sin(pit / 2) * cos(yaw / 2);
}

void Euler2DCM(const Eigen::Vector3d& euler, Eigen::Matrix3d& dcm){
    double rol = euler[ROL];
    double pit = euler[PIT];
    double yaw = euler[YAW];
    double c11 =  cos(pit) * cos(yaw);
    double c12 = -cos(rol) * sin(yaw) + sin(rol) * sin(pit) * cos(yaw);
    double c13 =  sin(rol) * sin(yaw) + cos(rol) * sin(pit) * cos(yaw);
    double c21 =  cos(pit) * sin(yaw);
    double c22 =  cos(rol) * cos(yaw) + sin(rol) * sin(pit) * sin(yaw);
    double c23 = -sin(rol) * cos(yaw) + cos(rol) * sin(pit) * sin(yaw);
    double c31 = -sin(pit);
    double c32 =  sin(rol) * cos(pit);
    double c33 =  cos(rol) * cos(pit);

    dcm <<  c11, c12, c13,
            c21, c22, c23,
            c31, c32, c33;
}

void DCM2Euler(const Eigen::Matrix3d& dcm, Eigen::Vector3d& euler){
    // 范围
    // rol: -pi~pi
    // pit: -pi/2~pi/2
    // yaw: -pi~pi
    double rol = atan2( dcm(2, 1), dcm(2, 2));
    double pit = atan2(-dcm(2, 0), sqrt(dcm(2, 1) * dcm(2, 1) + dcm(2, 2) * dcm(2, 2)));
    double yaw = atan2( dcm(1, 0), dcm(0, 0));
    
    euler << yaw, pit, rol;
}

void Quat2DCM(const Eigen::Quaterniond& quat, Eigen::Matrix3d& dcm) {
    double q0 = quat.w();
    double q1 = quat.x();
    double q2 = quat.y();
    double q3 = quat.z();
    dcm <<  q0*q0 + q1*q1 - q2*q2 - q3*q3   , 2*(q1*q2-q0*q3)               , 2*(q1*q3+q0*q2)               ,
            2*(q1*q2+q0*q3)                 , q0*q0 - q1*q1 + q2*q2 - q3*q3 , 2*(q2*q3-q0*q2)               ,
            2*(q1*q3-q0*q2)                 , 2*(q2*q3+q0*q1)               , q0*q0 - q1*q1 - q2*q2 + q3*q3 ;
}
// q_ne转换为经纬度
void Quat2Pos(const Eigen::Quaterniond& quat, Eigen::Vector3d& pos){
    pos[LAT] = -2 * atan2(quat.y(), quat.w()) - PI / 2;
    pos[LON] = 2 * atan2(quat.z(), quat.w());
}

// 经纬度转换为q_ne
void Pos2Quat(const Eigen::Vector3d& pos, Eigen::Quaterniond& quat){
    quat.w() =  cos(-PI / 4 - pos[LAT] / 2) * cos(pos[LON] / 2);
    quat.x() = -sin(-PI / 4 - pos[LAT] / 2) * sin(pos[LON] / 2);
    quat.y() =  sin(-PI / 4 - pos[LAT] / 2) * cos(pos[LON] / 2);
    quat.z() =  cos(-PI / 4 - pos[LAT] / 2) * sin(pos[LON] / 2);
}

// 获取输入向量的反对称矩阵
Eigen::Matrix3d getAntiSymmetricMatrix(const Eigen::Vector3d& vec){
    Eigen::Matrix3d m;
    m <<          0, -vec[2],  vec[1],
             vec[2],       0, -vec[0],
            -vec[1],  vec[0],       0;
    return m;
}

// 计算当前纬度的子午圈曲率半径Rm和卯酉圈曲率半径Rn
void getRmRn(double lat, double& Rm, double& Rn){
    Rm = Ra * (1 - E2) / pow(1 - E2 * sin(lat) * sin(lat), 1.5);
    Rn = Ra / sqrt(1 - E2 * sin(lat) * sin(lat));
}

// 计算当前位置重力加速度
Eigen::Vector3d getG(const Eigen::Vector3d& pos){
    // 计算当前纬度的重力加速度
    double g_lat    = (Ra * GA * pow(cos(pos[LAT]), 2) + Rb * GB * pow(sin(pos[LAT]), 2))
                    / sqrt(pow(Ra, 2) * pow(cos(pos[LAT]), 2) + pow(Rb, 2) * pow(sin(pos[LAT]), 2));
    double m = pow(OMEGA_E, 2) * pow(Ra, 2) * Rb / GM;
    // 计算当前位置的重力加速度
    Eigen::Vector3d g_n;
    g_n <<  0, 0,  g_lat * (1 - 2.0 / Ra * (1 + F + m - 2 * F * pow(sin(pos[LAT]), 2)) * pos[ALT]
            + 3 * pow(pos[ALT], 2) / pow(Ra, 2));
    // g_n <<  0, 0,  9.80665;
    return g_n;
}

};

#endif